﻿module Physics.Chapter4.Geometry

open Casanova
open Casanova.Input
open Casanova.Core
open Casanova.Utilities
open Casanova.Math
open Casanova.Game
open Casanova.Drawing
open Casanova.StandardLibrary
open Casanova.StandardLibrary.Core

let LineToLineDistance(line1Point1 : Vector3<m>, line1Point2 : Vector3<m>, line2Point1 : Vector3<m>, line2Point2 : Vector3<m>) =

   // Algorithm is ported from the C algorithm of 
   // Paul Bourke at http://local.wasp.uwa.edu.au/~pbourke/geometry/lineline3d/
 
   let p1 = line1Point1;
   let p2 = line1Point2;
   let p3 = line2Point1;
   let p4 = line2Point2;
   let p13 = p1 - p3;
   let p43 = p4 - p3;
   let p21 = p2 - p1;
 
   if (p43.Length < 0.01f<m>) then
      None
   elif (p21.Length < 0.01f<m>) then
      None
   else 
     let d1343 = p13.X * p43.X + p13.Y * p43.Y + p13.Z * p43.Z;
     let d4321 = p43.X * p21.X + p43.Y * p21.Y + p43.Z * p21.Z;
     let d1321 = p13.X * p21.X + p13.Y * p21.Y + p13.Z * p21.Z;
     let d4343 = p43.X * p43.X + p43.Y * p43.Y + p43.Z * p43.Z;
     let d2121 = p21.X * p21.X + p21.Y * p21.Y + p21.Z * p21.Z;
 
     let denom = d2121 * d4343 - d4321 * d4321;
     if (abs(denom) < 0.01f<_>) then
        None
     else
       let numer = d1343 * d4321 - d1321 * d4343;
 
       let mua = numer / denom;
       let mub = (d1343 + d4321 * (mua)) / d4343;
 
       if mua >= 0.0f && mua <= 1.0f &&
          mub >= 0.0f && mub <= 1.0f then
         let resultSegmentPoint1 = p1 + p21 * mua
         let resultSegmentPoint2 = p3 + p43 * mub

         Some(resultSegmentPoint1, resultSegmentPoint2)
       else
         None


type Face = {
  Vertices      : List<Vertex>
  Center        : Vector3<m>
  Tangent       : Vector3<1>
  Normal        : Vector3<1>
  Binormal      : Vector3<1>
  Id            : int
} with 
    member face.Edges = 
      seq{
        for i = 0 to face.Vertices.Length - 1 do
          yield face.Vertices.[i], face.Vertices.[(i+1) % face.Vertices.Length]
      }

and Edge = {
  Vertices      : List<Vertex>
  Center        : Vector3<m>
  Id            : int
} with member this.D = Vector3.Normalize(this.Vertices.[1].Position - this.Vertices.[0].Position)

and Vertex = {
  Position      : Vector3<m>
  IncidentEdges : ResizeArray<Edge>
  IncidentFaces : ResizeArray<Face>
  Id            : int
}

and CollisionType<'rigid_body> = FaceFace of 'rigid_body * 'rigid_body * Face * Face * Vertex * 
                                              List<Vector3<m> * // position
                                                   Vector3<1> * Vector3<1> * Vector3<1> * // TNB
                                                   float32<m>> * // depth
                                              List<Vector3<m> * Vector3<1> * Vector3<1> * Vector3<1> * float32<m>> | EdgeEdge of 'rigid_body * 'rigid_body * Edge * Edge * Vector3<m> * Vector3<m> | NoCollision
    with member coll.Vertices = 
          match coll with
          | NoCollision -> []
          | FaceFace(_,_,_,_,_,vs1,vs2) -> (vs1 |> List.map (fun (x,t,n,b,d) -> x)) @ (vs2 |> List.map (fun (x,t,n,b,d) -> x))
          | EdgeEdge(_,_,_,_,v1,v2) -> [v1;v2]
         member coll.Description = 
          match coll with
          | NoCollision -> "none"
          | FaceFace(_,_,_,_,_,_,_) -> "face"
          | EdgeEdge(_,_,_,_,_,_) -> "edge"

let CreateCube(size : Vector3<m>) = 
  let id = var 0
  let v = 
    [|  for i = 0 to 1 do
          let x = float32(i * 2 - 1) * size.X
          yield 
            [|  for j = 0 to 1 do
                  let y = float32(j * 2 - 1) * size.Y
                  yield 
                    [|  for l = 0 to 1 do
                          let z = float32(l * 2 - 1) * size.Z
                          yield { Position = Vector3<m>.Create(x, y, z)
                                  IncidentEdges = ResizeArray()
                                  IncidentFaces = ResizeArray()
                                  Id            = !id }
                          do id := !id + 1
                    |]
            |]
    |]

  let all_vertices = seq{ for i in v do for j in i do yield! j }

  let get_vertices vs = [ for (i,j,l) in vs do yield v.[i].[j].[l] ]
  let id = var 0
  let make_face vs = 
    let vs = get_vertices vs
    let T = Vector3.Normalize(vs.[1].Position - vs.[0].Position)
    let AC = vs.[2].Position - vs.[0].Position
    let N = Vector3.Normalize(Vector3<m>.Cross(AC, T * 1.0f<_>))
    let B = Vector3<1>.Cross(T,N)
    let cx = vs |> Seq.averageBy (fun v -> v.Position.X)
    let cy = vs |> Seq.averageBy (fun v -> v.Position.Y)
    let cz = vs |> Seq.averageBy (fun v -> v.Position.Z)
    let face =  {
                  Vertices = vs
                  Tangent  = T
                  Normal   = N
                  Binormal = B
                  Center   = Vector3.Create(cx,cy,cz)
                  Id       = !id 
                }
    do id := !id + 1
    for v in vs do v.IncidentFaces.Add face
    face
  let id = var 0
  let make_edge vs = 
    let vs = get_vertices vs
    let cx = vs |> Seq.averageBy (fun v -> v.Position.X)
    let cy = vs |> Seq.averageBy (fun v -> v.Position.Y)
    let cz = vs |> Seq.averageBy (fun v -> v.Position.Z)
    let edge = { Edge.Vertices = vs; Center = Vector3.Create(cx,cy,cz); Id = !id  }
    do id := !id + 1
    for v in vs do v.IncidentEdges.Add edge
    edge

  let faces = 
    [
      [(0,0,0); (1,0,0); (1,1,0); (0,1,0)]
      [(0,1,0); (1,1,0); (1,1,1); (0,1,1)]
      [(1,1,1); (1,0,1); (0,0,1); (0,1,1)]
      [(1,0,0); (0,0,0); (0,0,1); (1,0,1)]
      [(1,0,0); (1,0,1); (1,1,1); (1,1,0)]
      [(0,0,0); (0,1,0); (0,1,1); (0,0,1)]
    ]

  let edges = 
    [
      for [a;b;c;d] in faces do
        yield [a;b]
        yield [b;c]
        yield [c;d]
        yield [d;a]
    ] |> List.map make_edge
  let faces = faces |> List.map make_face

  let vertices = [ for i in v do for j in i do for l in j do yield l ]
  vertices, faces, edges


let find_contact_points use_compenetrating_edges (face1:Face) (face2:Face) (from_local_space1:Vector3<m> -> Vector3<m>) (from_local_space_normal1:Vector3<1> -> Vector3<1>)
                        (from_local_space2:Vector3<m> -> Vector3<m>) (from_local_space_normal2:Vector3<1> -> Vector3<1>) =
  let plane_dist (T,N,B,d) v = Vector3<m>.Dot(v,N) - d
  let clip planes v = planes |> Seq.forall (fun (T,N,B,d) -> plane_dist (T,N,B,d) v >= -0.1f<m>)

  let get_clipping_planes (face:Face) = 
    let edges = face.Edges
    let normal = face.Normal
    [
      let l = edges |> Seq.length
      for i = 0 to l - 1 do
        let v1,v2 = edges |> Seq.nth i
        let v2,v3 = edges |> Seq.nth ((i+1) % l)
        let A,B,C = from_local_space1(v1.Position), from_local_space1(v2.Position), 
                    from_local_space1(v1.Position) + from_local_space_normal1(normal) * 1.0f<m>
        let T = Vector3.Normalize(B - A)
        let N = Vector3.Normalize(Vector3<m>.Cross(C - A, T * 1.0f<_>))
        let B = Vector3<1>.Cross<_>(T,N)
        let d = Vector3<m>.Dot(A, N)
        if plane_dist (T,N,B,d) (from_local_space1 v3.Position) <= 0.0f<m> then 
          yield T,-N,B,-d
        else
          yield T,N,B,d
    ]

  // extend edges of reference face to planes
  let clipping_planes = get_clipping_planes face1

  // find edges of incident face
  let incident_edges = 
    [
      for v1,v2 in face2.Edges do
        yield from_local_space2 v1.Position, from_local_space2 v2.Position
    ]

  let T = from_local_space_normal1 face1.Tangent
  let N = from_local_space_normal1 face1.Normal
  let B = from_local_space_normal1 face1.Binormal

  // clip them against planes
  let clipped_edge_extremes =
    [
      let d = Vector3<m>.Dot(N, from_local_space1 face1.Center)
      for v1,v2 in incident_edges do
        let v12 = v2 - v1
        let denom = Vector3<m>.Dot(v12, N)
        let a = (d - Vector3<m>.Dot(v1,N)) / (max 0.001f<m> denom)
        let v = v1 + v12 * a
        if abs denom > 0.01f<m> && a >= 0.0f && a <= 1.0f && clip clipping_planes v then
          let penetration = Vector3<m>.Dot(v,N) - d
          yield v,T,N,B,penetration
        elif use_compenetrating_edges then // edge lies entirely under (or over) face
          if plane_dist (T,N,B,d) v1 < 0.0f<m> then // edge lies under
            // find edge intersections with clipping planes and use those
            for clipping_plane in clipping_planes do
              let T',N',B',d' = clipping_plane
              let denom' = Vector3<m>.Dot(v12, N')
              if abs denom' > 0.01f<m> then
                let a' = (d' - Vector3<m>.Dot(v1,N')) / denom'
                let v = v1 + v12 * a'
                if a' >= 0.0f && a' <= 1.0f && clip clipping_planes v then
                  let penetration = Vector3<m>.Dot(v,N) - d
                  yield v,T,N,B,penetration
    ]

  // these are all contact points
  let v0 = from_local_space1(face1.Vertices.[0].Position)
  [
    for v in face2.Vertices do
      let v = from_local_space2 v.Position
      let d = Vector3<m>.Dot(v-v0,N)
      if d < 0.1f<m> && clip clipping_planes v then
        yield v,T,N,B,d
  ] @ clipped_edge_extremes


let EdgeToEdgeCollisions(edges1 : List<Edge>, edges2 : List<Edge>, t : float32<s>, 
                         vel1 : Vector3<m/s>, vel2 : Vector3<m/s>, from_local_space1, 
                         from_local_space2) = 
  let from_local_space1 p = (from_local_space1 p) + vel1 * t
  let from_local_space2 p = (from_local_space2 p) + vel2 * t
  let edge_distances = 
    seq{
      for e1 in edges1 do
        for e2 in edges2 do
          match LineToLineDistance(from_local_space1(e1.Vertices.[0].Position), from_local_space1(e1.Vertices.[1].Position), 
                                   from_local_space2(e2.Vertices.[0].Position), from_local_space2(e2.Vertices.[1].Position)) with
          | Some(v1,v2) -> yield e1,e2,v1,v2,Vector3<m>.Distance(v1,v2)
          | None -> ()
    }
  if edge_distances |> Seq.isEmpty |> not then 
    let (e1,e2,v1,v2,d) = edge_distances |> Seq.minBy (fun (e1,e2,v1,v2,d) -> d)
    Some(v1,v2)
  else
    None


let FaceToFaceCollisions(f1 : Face, v2 : Vertex, is_immovable1, is_immovable2, t : float32<s>, vel1 : Vector3<m/s>, vel2 : Vector3<m/s>,
                         faces1 : List<Face>, faces2 : List<Face>, from_local_space1 : Vector3<m> -> Vector3<m>, 
                         from_local_space2 : Vector3<m> -> Vector3<m>, from_local_space_normal1 : Vector3<1> -> Vector3<1>, 
                         from_local_space_normal2 : Vector3<1> -> Vector3<1>) = 
  let from_local_space1 p = (from_local_space1 p) + vel1 * t
  let from_local_space2 p = (from_local_space2 p) + vel2 * t
  let face1 = f1
  let N1 = from_local_space_normal1 f1.Normal
  let face2 = v2.IncidentFaces |> Seq.minBy (fun f2 -> Vector3<1>.Dot(from_local_space_normal2 f2.Normal, N1))
  let N2 = from_local_space_normal2 face2.Normal

  let incident_vertices = find_contact_points (true) face1 face2 from_local_space1 from_local_space_normal1 
                                              from_local_space2 from_local_space_normal2

  let reference_vertices = find_contact_points (true) face2 face1 from_local_space2 from_local_space_normal2
                                              from_local_space1 from_local_space_normal1

  face1, face2, incident_vertices, reference_vertices
